#include "ros/ros.h"
#include "std_msgs/Header.h"
#include "manip_2l/JointAngles.h"

#include <sstream>

#define PI 3.1415926

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{

    ros::init(argc, argv, "traj_publisher");

    /**
     * NodeHandle is the main access point to communications with the ROS system.
     * The first NodeHandle constructed will fully initialize this node, and the last
     * NodeHandle destructed will close down the node.
     */
    ros::NodeHandle n;

    /**
     * Declare publisher, create publisher 'joint_angles' using the 'manip_2l::JointAngles'
     * message file from the 'manip_2l' package. The topic name is
     * 'joint_angles' and the size of the publisher queue is set to 1000.
     */
    ros::Publisher chatter_pub = n.advertise<manip_2l::JointAngles>("joint_angles", 1000);

    // Set the loop period. '10' refers to 10 Hz and the main loop repeats at 0.1 second intervals
    ros::Rate loop_rate(10);

    /**
     * t1: theta1
     * t2: theta2
     */
    float t1 = 0.0;
    float t2 = 0.0;

    int count = 100;
    for (int i = 0; i < count; i++)
    {
        if (!ros::ok())
        {
            return -1;
        }
        manip_2l::JointAngles msg;
        
        msg.header.seq = i;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "traj_pub";
        
        // ------------------------------------------
        // write your codes here


        // ------------------------------------------

        chatter_pub.publish(msg);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}